#include "os_config.h"
#if defined(__LINUX_OS__)
#include <unistd.h>
#include <stdio.h>
//#include <time.h>
#include <sys/time.h>
#include <sys/select.h>
#include <pthread.h>
#include <errno.h>

#include "error_functions.h"
static pthread_cond_t cond;
static pthread_mutex_t mutex;


void cutom_pthread_sleep_init(void)
{
   pthread_mutex_init(&mutex, NULL);
   pthread_cond_init(&cond, NULL);
}

void cutom_pthread_sleep_uninit(void)
{
   pthread_mutex_destroy(&mutex);
   pthread_cond_destroy(&cond);
}


void custom_pthread_sleep(unsigned int sec)
{
	struct timeval now;
	struct timespec outtime;
	
	pthread_mutex_lock(&mutex);

	gettimeofday(&now, NULL);
	outtime.tv_sec = now.tv_sec + sec;
	outtime.tv_nsec = now.tv_usec * 1000;
	pthread_cond_timedwait(&cond, &mutex, &outtime);

	pthread_mutex_unlock(&mutex);

}


void sleep_time_cust(const int secs, const int milli_secs)
{
	int ret;
	struct timeval tv;

	tv.tv_sec = secs;
	tv.tv_usec = (milli_secs * 1000);
	ret = select(0, NULL, NULL, NULL, &tv);
	if(ret == -1)
		perror("sleep_time_cust err");
}

void sleep_time_ms(const int ms)
{
	int sec = 0;
	int new_ms = ms;
	if(ms >= 1000){
		sec = ms /1000;
		new_ms = ms % 1000;
	}

	sleep_time_cust(sec,new_ms);
		
}


void sleep_time_us(const long us)
{
	int ret;
	struct timeval tv;

	tv.tv_sec = 0;
	tv.tv_usec = us;
	ret = select(0, NULL, NULL, NULL, &tv);
	if(ret == -1)
		perror("sleep_time_us err");
}

unsigned sleep_cust(unsigned nsec)
{
	int n;
	unsigned slept;
	time_t start,end;
	struct timeval tv;

	tv.tv_sec = nsec;
	tv.tv_usec = 0;
	time(&start);
	n = select(0,NULL,NULL,NULL,&tv);
	if(n == 0)
		return 0;
	time(&end);
	slept = end - start;
	if(slept >= nsec)
		return 0;
	return nsec - slept;
}
#endif
//#define SLEEP_TEST_TRACE
#if defined(__LINUX_OS__)

int  sleep_ms_v2(unsigned long ms)
{
	#ifdef SLEEP_TEST_TRACE
	struct timeval start, finish;
	#endif
	struct timespec request, remain;
	
	int s;
	
	int sec = 0;
	int new_ms = ms;
	if(ms >= 1000){
		sec = ms /1000;
		new_ms = ms % 1000;
	}

	request.tv_sec = sec;
	request.tv_nsec = new_ms * 1000000;

	#ifdef SLEEP_TEST_TRACE
	if (gettimeofday(&start, NULL) == -1)
		errExit("gettimeofday");
	#endif

	for (;;) {
		s = nanosleep(&request, &remain);
		if (s == -1 && errno != EINTR){
			perror("nanosleep");
			return -1;//errExit("nanosleep");
		}
			
		#ifdef SLEEP_TEST_TRACE	
		if (gettimeofday(&finish, NULL) == -1)
			errExit("gettimeofday");
		printf("Slept for: %9.6f secs\n", finish.tv_sec - start.tv_sec +
				(finish.tv_usec - start.tv_usec) / 1000000.0);
		#endif
		
		if (s == 0)
			break; /* nanosleep() completed */
		
		#ifdef SLEEP_TEST_TRACE
		printf("Remaining: %2ld.%09ld\n", (long) remain.tv_sec,
					remain.tv_nsec);
		#endif
		request = remain; /* Next sleep is with remaining time */
	}

	return 0;
}

int  sleep_us_v2(unsigned long us)
{
	#ifdef SLEEP_TEST_TRACE
	struct timeval start, finish;
	#endif
	struct timespec request, remain;
	
	int s;
	
	int sec = 0;
	int new_us = us;
	if(us >= 1000000){
		sec = us /1000000;
		new_us = us % 1000000;
	}

	request.tv_sec = sec;
	request.tv_nsec = new_us * 1000;

	#ifdef SLEEP_TEST_TRACE
	if (gettimeofday(&start, NULL) == -1)
		errExit("gettimeofday");
	#endif

	for (;;) {
		s = nanosleep(&request, &remain);
		if (s == -1 && errno != EINTR){
			perror("nanosleep");
			return -1;//errExit("nanosleep");
		}
			
		#ifdef SLEEP_TEST_TRACE	
		if (gettimeofday(&finish, NULL) == -1)
			errExit("gettimeofday");
		printf("Slept for: %9.6f secs\n", finish.tv_sec - start.tv_sec +
				(finish.tv_usec - start.tv_usec) / 1000000.0);
		#endif
		
		if (s == 0)
			break; /* nanosleep() completed */
		
		#ifdef SLEEP_TEST_TRACE
		printf("Remaining: %2ld.%09ld\n", (long) remain.tv_sec,
					remain.tv_nsec);
		#endif
		request = remain; /* Next sleep is with remaining time */
	}

	return 0;
}

void test_sleep_ms_v2(void)
{
	sleep_ms_v2(1000);

	sleep_ms_v2(200);

	sleep_ms_v2(3500);

	sleep_us_v2(500);
}
#endif

#if defined(__RTT_OS__)
unsigned sleep_cust(unsigned nsec)
{
	delay_ms(1000*nsec);
	return 0;
}
#endif